#include "qrender/q3ds_model.h"

#include <fstream>

#include <QOpenGLContext>
#include <QOpenGLFunctions>

#include <QDebug>
#include <QDir>
#include <QtMath>

#include "q3ds_process.h"

Q3dsModel::Q3dsModel()
{
    // demo 默认 DH 参数
    std::unordered_map<std::string, std::vector<double>> dh;
    dh["a"] = std::vector<double>{ 0, 0, 0.408, 0.376, 0, 0 };
    dh["alpha"] =
        std::vector<double>{ 0, -M_PI / 2., M_PI, M_PI, -M_PI / 2., M_PI / 2. };
    dh["d"] = std::vector<double>{ 0.122, 0.1215, 0, 0, 0.1025, 0.094 };
    dh["theta"] = std::vector<double>{ M_PI, -M_PI / 2., 0, -M_PI / 2., 0, 0 };

    reload(QStringList() << ".", "demo", dh);
}

bool Q3dsModel::reload(
    const QStringList &dirs, const QString &robot_type,
    const std::unordered_map<std::string, std::vector<double>> &dh)
{
    if (dirs_ == dirs && robot_type_ == robot_type && dh_ == dh) {
        return true;
    }
    dirs_ = dirs;
    robot_type_ = robot_type;
    dh_ = dh;

    // Create and load the link model
    QString q3ds_dir;
    for (auto &_dir : dirs) {
        auto dir_tmp =
            _dir + QString("/3ds_model_description/meshes/%1").arg(robot_type);
        if (QDir(dir_tmp).exists()) {
            q3ds_dir = dir_tmp;
            break;
        }
    }

    if (q3ds_dir.isEmpty()) {
        qWarning() << "Cannot load models for " << robot_type;
        return false;
    }

    auto add_link = [this, &q3ds_dir](const QString &link, double a,
                                      double alpha, double d, double theta) {
        links_.append(std::make_shared<Q3dsProcess>(
            QString("%1/visual/%2.3ds").arg(q3ds_dir, link), a, alpha, d,
            theta));
    };

    auto link_names = std::vector<std::string>{ "link1", "link2", "link3",
                                                "link4", "link5", "link6" };
    auto a = dh.at("a");
    auto alpha = dh.at("alpha");
    auto d = dh.at("d");
    auto theta = dh.at("theta");

    links_.clear();
    meshs_.clear();
    add_link("link0", 0., 0, 0, 0);
    for (int i = 0; i < 6; i++) {
        add_link(link_names[i].c_str(), a[i] * 1000.,
                 qRadiansToDegrees(alpha[i]), d[i] * 1000.,
                 qRadiansToDegrees(theta[i]));
    }
    for (auto &l : links_) {
        l->loadModel();
    }

    meshs_changed_ = true;

    return true;
}

void Q3dsModel::setJointPositions(const std::vector<double> &joint_positions)
{
    angles_.resize(joint_positions.size());
    for (size_t i = 0; i < angles_.size(); i++) {
        angles_[i] = qRadiansToDegrees(joint_positions[i]);
    }

    angles_chagned_ = true;
}

QVector<Q3dsMesh *> Q3dsModel::getMeshs()
{
    if (angles_.size() != (links_.size() - 1)) {
        return {};
    }

    // 如果数据获取过，则不再重复获取
    // 关节数据变化时，setJointPoisitions 中会立即变化数据
    if (meshs_.size() == links_.size() && !angles_chagned_) {
        return meshs_;
    }
    meshs_.clear();

    // 模型偏移
    QMatrix4x4 translation_matrix;
    translation_matrix.rotate(-90.0, 1.0, 0.0, 0.0); // Y 轴向上
    translation_matrix.rotate(-90.0, 0.0, 0.0, 1.0);

    // 底座
    auto mesh = links_[0]->getMesh(0, translation_matrix);
    meshs_ << mesh;

    // 关节
    for (size_t i = 1; i <= angles_.size(); i++) {
        mesh = links_[i]->getMesh(angles_[i - 1], translation_matrix);
        meshs_ << mesh;
    }

    angles_chagned_ = false;
    meshs_changed_ = false;

    return meshs_;
}

bool Q3dsModel::meshsChanged()
{
    return meshs_changed_;
}
